#include "KDTree.h"

#include "skeleton_compare.h"
#include <QMessageBox>
#include "CurveskelModel.h"
#include "CurveskelHelper.h"
#include "StarlabDrawArea.h"

std::vector<double> toKDPoint(const CurveskelTypes::Point & from){
    std::vector<double> p(3, 0.0);
    p[0] = from.x(); p[1] = from.y(); p[2] = from.z();
    return p;
}

void skeleton_compare::applyFilter(RichParameterSet* params){
    drawArea()->deleteAllRenderObjects();
    
    bool show_measurements = params->getBool("Show measurements");
    
    //CurveskelModel* src = document()->getModel<CurveskelModel>(params->getString("Target Skeleton"));
    // == Temporary code since 'getModel' is missing
    QList<Model*> allModels = document()->models();
    allModels.removeAll(document()->selectedModel());
    if(allModels.size() < 1) return;
    CurveskelModel* src = qobject_cast<CurveskelModel*>(allModels.first());
    // == End of temp code

    CurveskelModel* target = qobject_cast<CurveskelModel*>( document()->selectedModel() );
    if(!target) throw StarlabException("Must be a skeleton model");
    
    CurveskelModel::Vertex_property<CurveskelTypes::Point> src_pnts = src->vertex_property<CurveskelTypes::Point>("v:point");
    CurveskelModel::Vertex_property<CurveskelTypes::Point> target_pnts = target->vertex_property<CurveskelTypes::Point>("v:point");
    
    // Construct a kd-tree
    std::vector< vector<double> > src_points;
    foreach(CurveskelTypes::Vertex v, src->vertices())
        src_points.push_back( toKDPoint(src_pnts[v]) );
    KDTree src_tree(src_points);

    // Common variables
    int closeidx = -1;
    double di = 0.0;

    // Compare target points
    double avgDifference=0.0;
    {
        foreach(CurveskelTypes::Vertex v, target->vertices()){
            // Locate closest point, return distance
            src_tree.closest_point( toKDPoint( target_pnts[v] ), closeidx, di);
            avgDifference += di;
            
            if(show_measurements)
                drawArea()->drawSegment( target_pnts[v], src_pnts[ CurveskelTypes::Vertex(closeidx) ], 2, Qt::blue);
        }
        avgDifference /= target->n_vertices();
        double bbox_diag = src->bbox().size().length();
        avgDifference /= bbox_diag;
    }

    /// @todo use dialog of sorts..
    qDebug() <<"One sided distance (bbox-norm)" << target->name << "=> " << src->name << avgDifference; 
}

Q_EXPORT_PLUGIN(skeleton_compare)
